import os
import launch
import launch_ros.actions
from launch_ros.substitutions import FindPackageShare
import xacro
from launch.substitutions import PathJoinSubstitution
from launch.substitutions import Command
from ament_index_python.packages import get_package_share_directory
from launch.actions import TimerAction
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessStart

XACRO_FILE_NAME = 'rrbot.xacro'


def generate_launch_description():
  
    pkg_share = FindPackageShare('rrbot_description').find('rrbot_description')
    
    urdf_dir = os.path.join(pkg_share, 'urdf')
    xacro_file = os.path.join(urdf_dir, XACRO_FILE_NAME)
    doc = xacro.process_file(xacro_file)
    robot_desc = doc.toprettyxml(indent='  ')
    params = {'robot_description': robot_desc}
    
    robot_description_content = Command(['xacro', ' ', os.path.join(pkg_share, 'urdf', XACRO_FILE_NAME)])
    robot_description = {"robot_description": robot_description_content}
    
    rsp = launch_ros.actions.Node(package='robot_state_publisher',
                                  executable='robot_state_publisher',
                                  output='both',
                                  parameters=[params])
    
    rviz_config_file = PathJoinSubstitution(
      [FindPackageShare('rrbot_description'), "rviz", "view.rviz"]
    )
    
    rviz2_node = launch_ros.actions.Node(package='rviz2',
                                         executable='rviz2',
                                         name='rviz2',
                                         output='log',
                                         arguments=['-d', rviz_config_file])
    
    controller_manager_node = launch_ros.actions.Node(package='controller_manager',
                                                      executable='ros2_control_node',
                                                      parameters=[{'robot_description': Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])},
                                                                    os.path.join(get_package_share_directory('rrbot_description'), 'config', 'rrbot_controllers.yaml')]
    )
    delayed_controller_manager_node = TimerAction(period=3.0, actions=[controller_manager_node])
    
    
    joint_state_broadcaster_node = launch_ros.actions.Node(package='controller_manager',
                                                           executable='spawner',
                                                           arguments=['joint_state_broadcaster', 
                                                                      '--controller-manager', 
                                                                      '/controller_manager'],
    )
    
    delayed_joint_state_broadcaster_node = RegisterEventHandler(
      event_handler=OnProcessStart(
        target_action=controller_manager_node,
        on_start=[joint_state_broadcaster_node],
      )
    )
    
    joint_trajectory_controller_node = launch_ros.actions.Node(package='controller_manager',
                                                               executable='spawner',
                                                               arguments=['joint_trajectory_controller',
                                                                          '--controller-manager',
                                                                          '/controller_manager'],
    )
    
    delayed_joint_trajectory_controller_node = RegisterEventHandler(
      event_handler=OnProcessStart(
        target_action=controller_manager_node,
        on_start=[joint_trajectory_controller_node],
      )
    )
                                         

    return launch.LaunchDescription([rsp,
                                     rviz2_node,
                                     delayed_controller_manager_node,
                                     delayed_joint_state_broadcaster_node,
                                     delayed_joint_trajectory_controller_node])